/*
 * OptitrackUAVOperator.h
 *
 *  Created on: Oct 25, 2016
 *      Author: throne
 */

#ifndef PX4_OPERATOR_SRC_OPTITRACK_UAV_OPERATOR_OPTITRACK_UAV_OPERATOR_H_
#define PX4_OPERATOR_SRC_OPTITRACK_UAV_OPERATOR_OPTITRACK_UAV_OPERATOR_H_

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>

#include "eigen_conversions/eigen_msg.h"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/ParamGet.h>
#include <mavros_msgs/ParamSet.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <fstream>
#include <iostream>
#include <string>
//#include "display_planner/DisplayPlanner.h"
#include "../display_planner/DisPlayPlanner.h"

using Eigen::Affine3d;
using Eigen::Vector3d;
using Eigen::Matrix3d;
using std::string;

class OptitrackUAVOperator {
friend class UAVLeader;
struct Status
{
	Affine3d pose;
	bool armed;
	bool connected;
	bool opti_pose_valid;
};

public:
	Status current_status;

private:
	const double OPTITRACK_TIMEOUT = 0.2;

	Affine3d pose_opti_enu;
	Affine3d pose_uav_rb;
	string rigidbody_name;
	string mavros_namespace;
	mavros_msgs::State state;
	Affine3d opti_pose;
	Affine3d mavros_pose;
	Affine3d target_pose;

	ros::Subscriber mocap_vrpn_sub;

	//2.24
//	ros::Subscriber mocap_vrpn_STANDERD_sub;
//	string STANDERD_num;

	//
	ros::Subscriber mavros_state_sub;
	ros::Subscriber mavros_pose_sub;

	ros::Publisher uav_pose_pub;
	ros::Publisher position_target_pub;

	ros::Publisher velocity_target_pub;
	ros::Publisher angle_target_pub;



    ros::ServiceClient arming_client;
    ros::ServiceClient set_mode_client;
    ros::ServiceClient takeoff_client;
    ros::Publisher acc_target_pub;

    ros::ServiceClient para_get;
    ros::ServiceClient para_set;

    double last_optitrack_timestamp;
    double current_timestamp;
    bool opti_pose_valid;

public:
    double label;//
    PathPlanner path;

public:
    bool init(ros::NodeHandle& nh, string& rigidbody_name_, string& mavros_namespace_, double num);

	void optitrack_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);

	void optitrack_pose_standerd_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
	void mavros_state_callback(const mavros_msgs::State::ConstPtr& msg);
	void mavros_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);

	void send_uav_pose();
	void send_position_target(const Vector3d& position_target, double yaw);
	void send_position_target(const Affine3d& pose_target);
	void send_velocity_target(const Vector3d& velocity_target);
	void send_angle_target(const Affine3d& angle_target);
public:
	void spinonce();

	bool is_connected();
	bool set_offboard();
	bool arm();
	bool disarm();
	bool takeoff(int choice);

	//
	bool display(double time_now, Affine3d pose, Affine3d* out_matrix, ifstream& path_ifs, bool start);

	static void xyzyaw_to_matrix(const Vector3d& position, double yaw, Affine3d* out_matrix);

	static void xyzyaw_to_matrix(const Vector3d& position, const Vector3d& euler, Affine3d* out_matrix);

	static void matrix_to_xyzyaw(const Affine3d& matrix, Vector3d* position, double* yaw);



	OptitrackUAVOperator();
	virtual ~OptitrackUAVOperator();
};

#endif /* PX4_OPERATOR_SRC_OPTITRACK_UAV_OPERATOR_OPTITRACK_UAV_OPERATOR_H_ */
